// PhaseSpace implementation
//////////////////////////////////////////////////////////////////

# include "PhaseSpace.h"
# define CONV 0.01745329252

//////////////////////////////////////////////////////////////////
// Phase Space : constructors/destructors
//////////////////////////////////////////////////////////////////


PhaseSpace::~PhaseSpace() //look up tilde
{
if(DMA)
	{
	deleteA(Position);
	deleteA(Momentum);
	deleteA(Derivative);
	}
}


//////////////////////////////////////////////////////////////////
// Phase Space : functions
//////////////////////////////////////////////////////////////////

// What is the input function?
// method newA? Probabilities?
// DMA?
// This method initialises the number of degrees of freedom, position, momentum
// and derivative.
void PhaseSpace::Initialise(int InputN, Function *InputFunction)
{
N              = InputN;
SampleFunction = InputFunction; 
newA(Position,   N);
newA(Momentum,   N);
newA(Derivative, N);  
DMA = true;	
}

// This method moves the position coordinates using the leap-frog algorithm
void PhaseSpace::IncrementPosition(double TimeStep)
{
for(int n=1;n<=N;n++)
	Position[n] += TimeStep*Momentum[n];
}

/*void PhaseSpace::IncrementPosition(double TimeStep, double *mass)
{

  for(int n=1;n<=N;n++)
  {
	  Position[n] += (TimeStep*Momentum[n])/mass[n];
    //Position[n] += TimeStep*Momentum[n];
  }
}*/

double PhaseSpace::IncrementPosition_observe(double TimeStep)
{
  mLength = 0.0;
  for(int n=1;n<=N;n++)
  {
    mIncreaseBy = TimeStep*Momentum[n];
	  Position[n] += mIncreaseBy;
    mLength += abs(mIncreaseBy);
  }
  return mLength;
}
	
void PhaseSpace::IncrementMomentum(double TimeStep)
{
  SampleFunction->BuildDerivative(Position, Derivative);

  for(int n=1;n<=N;n++)
  {
    Momentum[n] -= TimeStep*Derivative[n];
  }
}


double PhaseSpace::KineticEnergy(void)
{
double Value = 0;
for(int n=1;n<=N;n++)
	Value += Momentum[n]*Momentum[n];
return ( Value/2.0 );
}

/*double PhaseSpace::KineticEnergy(double *mass)
{
  double Value = 0;
  for(int n=1;n<=N;n++)
  {
	  Value += (Momentum[n]*Momentum[n])/mass[n];
    //Value += Momentum[n]*Momentum[n];
  }
  return ( Value/2.0 );
}*/


//////////////////////////////////////////////////////////////////
// Phase Space : overloaded operators
//////////////////////////////////////////////////////////////////

void PhaseSpace::operator=(PhaseSpace &X)
{
if(N!=X.N) 
	{
	cerr << "Problem with equality between two PhaseSpace objects\n";
	exit(1);
	}

CopyA(Position, X.Position, N);
CopyA(Momentum, X.Momentum, N);
}

void PhaseSpace::SafePositionPhaseSpace(ofstream &lStream)
{
  for (int i = 1; i <= N; i++)
    lStream << Position[i] << "  ";
  lStream << endl;
}

//////////////////////////////////////////////////////////////////
// Phase Space : friends
//////////////////////////////////////////////////////////////////


ostream &operator<<(ostream &os, const PhaseSpace &X)
{
os << "(\t";
for(int n=1;n<=X.N;n++)
	{
	os << X.Position[n];
	if(n!=X.N) 
		{
		os << ",\t";
		if(n%4==0) os << "\n\t";
		}
	}	
os << " )" << endl;

return ( os );
}


// to calculate the length between two configurations

double PhaseSpace::Difference_observe(double* configuration)
{
  mLength = 0.0;
  for(int n=1;n<=N;n++)
  {
    mLength += abs(Position[n] - configuration[n]);
  }
  return mLength;
}

/*
ostream &operator<<(ostream &os, const PhaseSpace &X)
{
double a = X.Position[1]/CONV;
double b = X.Position[2]/CONV;
double c = X.Position[3]/CONV;


a = fmod(a,360.0);
b = fmod(b,360.0);
c = fmod(c,360.0);


os << a << "\t" << b << "\t" << c << "\n";

return ( os );
}
*/







